#!/bin/bash

# 设置颜色代码
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
RED='\033[0;31m'
NC='\033[0m' # No Color

# 设置ROS环境
source /opt/ros/noetic/setup.bash
source /root/catkin_ws/devel/setup.bash

echo -e "${BLUE}=== 测试基于时间戳的目录结构功能 ===${NC}"
echo -e "${YELLOW}这个测试将运行NDT建图节点30秒，然后检查时间戳目录是否正确创建${NC}"

# 确保maps目录存在
MAPS_DIR="/root/catkin_ws/src/auto_navigation/maps"
mkdir -p $MAPS_DIR

# 清理之前的测试数据
echo -e "${YELLOW}清理之前的测试数据...${NC}"
rm -rf $MAPS_DIR/latest_mapping
rm -f $MAPS_DIR/kitti_best_map.pcd

# 启动NDT建图节点，运行30秒
echo -e "${GREEN}启动NDT建图节点，将运行30秒...${NC}"
echo -e "${YELLOW}地图将保存在基于时间戳的目录中: $MAPS_DIR/YYYYMMDD_HHMMSS/${NC}"

# 在后台运行roslaunch命令
roslaunch auto_navigation kitti_ndt_mapping_optimized.launch auto_save_interval:=10.0 &
LAUNCH_PID=$!

# 等待30秒
echo -e "${YELLOW}等待30秒...${NC}"
sleep 30

# 终止roslaunch进程
echo -e "${YELLOW}终止NDT建图节点...${NC}"
kill $LAUNCH_PID
sleep 5

# 检查目录结构
echo -e "${BLUE}=== 检查目录结构 ===${NC}"

# 检查latest_mapping符号链接是否存在
if [ -L "$MAPS_DIR/latest_mapping" ]; then
    echo -e "${GREEN}✓ latest_mapping符号链接已创建${NC}"
    
    # 获取符号链接指向的目录
    TIMESTAMP_DIR=$(readlink -f "$MAPS_DIR/latest_mapping")
    echo -e "${YELLOW}时间戳目录: $TIMESTAMP_DIR${NC}"
    
    # 检查时间戳目录中的文件
    echo -e "${BLUE}时间戳目录中的文件:${NC}"
    ls -la $TIMESTAMP_DIR
    
    # 检查kitti_latest_map.pcd符号链接
    if [ -L "$TIMESTAMP_DIR/kitti_latest_map.pcd" ]; then
        echo -e "${GREEN}✓ kitti_latest_map.pcd符号链接已创建${NC}"
        
        # 获取最新地图文件的大小
        MAP_SIZE=$(du -h $(readlink -f "$TIMESTAMP_DIR/kitti_latest_map.pcd") | cut -f1)
        echo -e "${YELLOW}最新地图文件大小: $MAP_SIZE${NC}"
    else
        echo -e "${RED}✗ kitti_latest_map.pcd符号链接未创建${NC}"
    fi
else
    echo -e "${RED}✗ latest_mapping符号链接未创建${NC}"
    
    # 列出maps目录中的所有内容
    echo -e "${BLUE}maps目录中的内容:${NC}"
    ls -la $MAPS_DIR
fi

echo -e "${BLUE}=== 测试完成 ===${NC}"
echo -e "${YELLOW}如果测试成功，您应该看到一个基于时间戳的目录和相应的符号链接${NC}"
echo -e "${YELLOW}您可以使用以下命令查看最新的地图:${NC}"
echo -e "${GREEN}/root/catkin_ws/src/auto_navigation/scripts/view_map.sh /root/catkin_ws/src/auto_navigation/maps/latest_mapping/kitti_latest_map.pcd${NC}" 